In [ ]:

    

%cd /home/moritz/py_lib/mmnotebooks/


    

In [ ]:

    

# Import libraries
#from models import bslip
import bslip
from bslip import (ICeuklid_to_ICcircle, ICcircle_to_ICeuklid, circ2normal_param, new_stridefunction, 
    vis_sim, stridefunction)
from copy import deepcopy # e.g. for jacobian calculation
import mutils.misc as mi
import mutils.io as mio

import sys
# define SLIP related functions
mi.run_nbcells('Walking SLIP Analysis 2 - C.ipynb', ['def_funs'])


    

In [ ]:

    

#define functions
def new_stridefunction_E(pbase, E_des, p_red):
    f = new_stridefunction(pbase)
    
    def innerfunction(IC_red):
        IC = ICe_to_ICc(IC_red, E_des, p_red[0], pbase['m'], l0 = pbase['lp1'][1])
        return f(IC, p_red)[[0,1,3,4]]
    return innerfunction

def new_stridefunction_E2(pbase, E_des, p_red):
    f = stridefunction(pbase)
    
    def innerfunction(IC_red):
        IC = ICe_to_ICc(IC_red, E_des, p_red[0], pbase['m'], l0 = pbase['lp1'][1])
        return f(IC, p_red)[[0,1,3,4]]
    return innerfunction


    

In [ ]:

    

c = mio.saveable() # config workspace
c.beta  = 5
c.beta_doc = 'beta angle of legs (in deg., 5 and 10 are available)'

c.point = 'B'
c.point_doc = 'which point of Geyers solutions (A,B,C)'

c.background_only = False
c.background_only_doc = 'display only background, no additional information'


    

In [ ]:

    

# old data
if c.point != 'B':
    dat1 = mio.mload('bslip_results/' + c.point + '_sols5_new.list')
    dat2 = mio.mload('bslip_results/' + c.point + '_sols10_new.list')
else:
    # new data
    dat1 = mio.mload('bslip_results/' + c.point + '_sols5_new_rev2.list')
    dat2 = mio.mload('bslip_results/' + c.point + '_sols10_new_rev2.list')


    

In [ ]:

    

# re-order data

# compute range of axes
dk1 = array([x[1][0] - x[1][1] for x in dat1]) / 1000.
da1 = array([x[1][2] - x[1][3] for x in dat1]) * 180 / pi

nk1 = len(set(dk1)) # number of different dk's
na1 = len(set(da1)) # number of different da's

ax_a1 = linspace(min(da1), max(da1), na1)
ax_k1 = linspace(min(dk1), max(dk1), nk1)

# delta_k
dk2 = array([x[1][0] - x[1][1] for x in dat2]) / 1000.
da2 = array([x[1][2] - x[1][3] for x in dat2]) * 180 / pi

nk2 = len(set(dk2)) # number of different dk's
na2 = len(set(da2)) # number of different da's

ax_a2 = linspace(min(da2), max(da2), na2)
ax_k2 = linspace(min(dk2), max(dk2), nk2)

print "re-ordering data into grid:"
n = 0
data1 = nan*zeros((nk1, na1))
data1ev = nan*zeros((nk1, na1))
data1_idx = nan*zeros((nk1, na1))
print "ws1:"
for idx, elem in enumerate(dat1):
    # compute index
    dk = (elem[1][0] - elem[1][1])/1000.
    da = (elem[1][2] - elem[1][3]) * 180. / pi
    idx_a = argmin(abs(ax_a1 - da))
    idx_k = argmin(abs(ax_k1 - dk))
    data1[idx_k, idx_a] = elem[2]
    data1ev[idx_k, idx_a] = max(abs(elem[3]))    
    data1_idx[idx_k, idx_a] = idx
    n+=1
    if n >= 500:       
        sys.stdout.write('.')        
        n=0
    
print "done"

n = 0
data2 = nan*zeros((nk2, na2))
data2ev = nan*zeros((nk2, na2))
data2_idx = nan*zeros((nk2, na2))
print "ws2:"
for idx, elem in enumerate(dat2):
    # compute index
    dk = (elem[1][0] - elem[1][1])/1000.
    da = (elem[1][2] - elem[1][3]) * 180/pi
    idx_a = argmin(abs(ax_a2 - da))
    idx_k = argmin(abs(ax_k2 - dk))
    data2[idx_k, idx_a] = elem[2]
    data2ev[idx_k, idx_a] = max(abs(elem[3]))
    data2_idx[idx_k, idx_a] = idx
    n+=1
    if n >= 500:       
        sys.stdout.write('.')        
        n=0
    

# for contour plots: separate positive and negative values    
ma1act = ma.array(data1, mask=(data1 > 0))
ma1bct = ma.array(data1, mask=(data1 < 0))

ma2act = ma.array(data2, mask=(data2 > 0))
ma2bct = ma.array(data2, mask=(data2 < 0))
    
    
print "done"


    

In [ ]:

    

#ct1 = ax1.contour(ax_k1 / 1000., ax_a1 * 180/pi, masked_array1ct, levels, linestyles='-',
                      #colors='k', )
#clabel(ct1, ct1.levels, inline=True, fmt='%.0f', fontsize=10)


    

In [ ]:

    

# define colormap
from matplotlib.colors import LinearSegmentedColormap

# continous black-red-black
cdict_1 = {'red' : ( (0.0, 0.0, 0.0),                     
                     (1.0, 0.35, 0.35) ),
           'green' : ( (0.0, 0.0, 0.0),                     
                     (1.0, 0.35, 0.35) ),
           'blue' : ( (0.0, 0.0, 0.0),                     
                     (1.0, 0.35, 0.35) ),
           }
map_dgrey = LinearSegmentedColormap('dgrey', cdict_1)

cdict_2 = {'red' : ( (0.0, 0.35, 0.35),                     
                     (1.0, 0.0, 0.0) ),
           'green' : ( (0.0, 0.35, 0.35),                     
                     (1.0, 0.0, 0.0) ),
           'blue' : ( (0.0, 0.35, 0.35),                     
                     (1.0, 0.0, 0.0) ),
           }
map_dgrey_i = LinearSegmentedColormap('dgrey_i', cdict_2)


    

In [ ]:

    

# compute takeoff height - touchdown height (existence condition)
idx = 0
yto = dat1[idx][0][0]
ytd = dat1[idx][1][6] * sin(dat1[idx][1][2])


    

In [ ]:

    

%config InlineBackend.close_figures = False


    

In [ ]:

    

close('all')
if c.point == 'A':
    myxlim=[-5,0]
    myylim=[-20,20]
elif c.point == 'B':
    myxlim=[-6,0]
    myylim=[-22,22]   
else:
    myxlim=[-12,0]
    myylim=[-2,2]
    
cmap = matplotlib.cm.Spectral
cmap.set_bad('w',1.)

if c.point=='A':
    #levels = [-200,-100,-50,-20,-10,10,20,50,100,200]
    fig = figure(figsize=(10,6))
elif c.point=='B':
    #levels = [-100,-20,-10,10,20,100]
    fig = figure(figsize=(5.5,4))    
else:
    #levels = [-150,-50,-20,-10,10,20,50,150]
    fig = figure(figsize=(5.5,4))


    

In [ ]:

    

clf()
evlvlsa = array([-150, -50, -20, -10, -5])
evlvlsb = -evlvlsa

# subplot1

subplot(1,2,1)
c1 = contour(ax_k1, ax_a1, ma1act.T, evlvlsa, cmap=map_dgrey)
c1.clabel(evlvlsa, fmt='%.0f', fontsize=10)
c1.set_clim(min(evlvlsa), max(evlvlsa))
c2 = contour(ax_k1, ax_a1, ma1bct.T, evlvlsb, cmap=map_dgrey_i)
c2.set_clim(min(evlvlsb), max(evlvlsb))
c2.clabel(evlvlsb, fmt='%.0f', fontsize=10)

p1 = imshow(data1ev.T[::-1,:], cmap=cm.jet, extent=(ax_k1.min(),ax_k1.max(),ax_a1.min(),ax_a1.max()), 
           aspect='auto', interpolation='none')
p1.set_clim((0,1.1))
ylim(myylim)
xlim(myxlim)
grid('on')

ax2 = subplot(1,2,2)

# subplot2

c2 = contour(ax_k2, ax_a2, ma2act.T[:,::1], evlvlsa, cmap=map_dgrey)
c2.clabel(evlvlsa, fmt='%.0f', fontsize=10)
c2.set_clim(min(evlvlsa), max(evlvlsa))
c2 = contour(ax_k2, ax_a2, ma2bct.T[:,::1], evlvlsb, cmap=map_dgrey_i)
c2.set_clim(min(evlvlsb), max(evlvlsb))
c2.clabel(evlvlsb, fmt='%.0f', fontsize=10)

p2 = imshow(data2ev.T[::-1,::-1], cmap=cm.jet, extent=(ax_k2.max(),ax_k2.min(),ax_a2.min(),ax_a2.max()), 
           aspect='auto', interpolation='none')
ax2.yaxis.set_ticks_position('right')
p2.set_clim((0,1.1))
ylim(myylim)
xlim(myxlim[::-1])
grid('on')


#colorbar()
subplots_adjust(wspace=.02)

savefig('img/new_f3_{}.pdf'.format(c.point))


    

In [ ]:

    

figure(figsize=(4,2))
data = linspace(0,1,50)[:,newaxis]
imshow(data)
p3 = imshow(data[::-1,:], cmap=cm.jet, extent=(0, 1, 0, 1), 
           aspect='auto', interpolation='none')
gca().yaxis.set_ticks_position('right')
p3.set_clim((0,1.1))
savefig('img/fig3_ev_cmap.pdf')
#ylim(myylim)
#xlim(myxlim[::-1])
#grid('on')


    

In [ ]:

    

# avoid scrolling


    

In [ ]:

    

colorlim = [-200,200]
if c.point in 'AB':
    myxlim=[-5,0]
    myylim=[-20,20]
else:
    myxlim=[-12,0]
    myylim=[-2,2]
    
cmap = matplotlib.cm.Spectral
cmap.set_bad('w',1.)

if c.point=='A':
    levels = [-200,-100,-50,-20,-10,10,20,50,100,200]
    fig = figure(figsize=(10,6))
elif c.point=='B':
    levels = [-100,-20,-10,10,20,100]
    fig = figure(figsize=(5.5,4))    
else:
    levels = [-150,-50,-20,-10,10,20,50,150]
    fig = figure(figsize=(5.5,4))


#--------------------- general frame plot for common title etc.
#
ax0 = fig.add_subplot(1,1,1,frameon=False)
ax0.set_xticks([])
ax0.set_yticks([])


#--------------------- first plot
ax1 = fig.add_subplot(1,2,1)
masked_array1 = ma.array (data1.T, mask=np.isnan(data1.T))
pcl1 = ax1.pcolor(ax_k1 / 1000., ax_a1 * 180 / pi, masked_array1, cmap=cmap)
pcl1.set_clim(colorlim)
if not c.background_only:
    masked_array1ct = ma.array (data1.T, mask=abs(data1.T)>max(abs(array(colorlim)))*1.2)
    ct1 = ax1.contour(ax_k1 / 1000., ax_a1 * 180/pi, masked_array1ct, levels, linestyles='-',
                      colors='k', )
    clabel(ct1, ct1.levels, inline=True, fmt='%.0f', fontsize=10)


#--------------------- second plot
ax2 = fig.add_subplot(1,2,2)
masked_array2 = ma.array (data2.T, mask=np.isnan(data2.T))
pcl2 = ax2.pcolor(ax_k2 / 1000., ax_a2 * 180/pi, masked_array2, cmap=cmap)
pcl2.set_clim(colorlim)
if not c.background_only:
    masked_array2ct = ma.array (data2.T, mask=abs(data2.T)>max(abs(array(colorlim)))*1.2)
    ct2 = ax2.contour(ax_k2 / 1000., ax_a2 * 180/pi, masked_array2ct, levels, linestyles='-',
                      colors='k', )
    clabel(ct2, ct2.levels, rightside_up=False, fmt='%.0f', fontsize=10)

#-------------------- colorbar frame
if not c.background_only:
    if c.point == 'A':
        ax3 = fig.add_axes([.8, .55, .08, .4])
        cb = colorbar(pcl2, cax=ax3)
        cb.set_ticks(linspace(colorlim[0], colorlim[1], 5))
        cb.set_label('walking radius [m]', fontsize=14)


#-------------------- labeling
ax0.set_xlabel('\n'*2 + r'$\Delta k$ [kN / m]', fontsize=14)
ax1.set_ylabel(r'$\Delta \alpha$[deg]', fontsize=14)
ax1.set_title(r'$\beta = 5^\circ $', fontsize=16)
ax2.set_title(r'$\beta = 10^\circ $', fontsize=16)


#-------------------- formatting
ax2.set_yticklabels([]) 
ax1.set_xlim(myxlim)
ax2.set_xlim(myxlim[::-1])
ax1.set_ylim(myylim)
ax2.set_ylim(myylim)
if c.point=='A':
    fig.subplots_adjust(hspace=0, wspace=0.025, bottom=.1, right=.875, left=.10)
else:
    fig.subplots_adjust(hspace=0, wspace=0.025, bottom=.2, right=.875, left=.15)
if not c.background_only:
    ax1.grid('on')
    ax2.grid('on')


#-------------------- save figures

#savefig('tmp/walking_radius_ak_' + c.point + '.png', dpi=100)
#savefig('tmp/walking_radius_ak_' + c.point + '.tiff', dpi=600)
#savefig('tmp/walking_radius_ak_' + c.point + '.svg', dpi=50)
if c.background_only:
    savefig('tmp/walking_radius_ak_' + c.point + '_bg.png')
else:
    savefig('tmp/walking_radius_ak_' + c.point + '.pdf')

pass


    

In [ ]:

    

# initialize model:
# select parameter
# select corresponding initial values
# find periodic solution (re-compute)

import bslip
p_red = array(bslip.demo_p_reduced)
E_des = 816.
p_base = bslip.demo_p
p_base['delta_beta'] = 0

selection = 3 # 1=A, 2=B, 3=C
bbeta = False # large beta (.1) or small beta (.05)?

# periodic solution 1:
if selection == 1:
    p_red[0] = p_red[1] = 14000
    p_red[2] = 69. * pi / 180.
    if not bbeta:
        p_red[3] = .05    
        IC0 = array([ 0.93358034,  0.43799844,  1.25842356,  0.94657321,  0.10969058]) # beta=.05
    else:
        p_red[3] = .1       
        IC0 = array([ 0.93358039,  0.45195548,  1.26003517,  0.94679076,  0.21853576]) # beta=.1

elif selection == 2:
    # periodic solution 2:  (asymmetric stance phase)
    p_red[0] = p_red[1] = 14000
    p_red[2] = 72.5 * pi / 180.
    if not bbeta:
        p_red[3] = 0.05
        IC0 = array([ 0.9308592,   0.3793116,   1.19155584,  0.9360028,   0.1597469 ]) # for beta=.05, alpha=72.5
    else:
        p_red[3] = .1
        IC0 = array([ 0.93011364,  0.39346135,  1.19332777,  0.93554008,  0.31541887]) # for beta=.1, alpha=72.5
    
elif selection == 3:
    # periodic solution 3:
    p_red[0] = p_red[1] = 20000
    p_red[2] = 76.5 * pi / 180.
    if not bbeta:
        p_red[3] = 0.05
        IC0 = array([ 0.97236992,  0.17616847,  1.07200705,  0.97370148,  0.22353624]) # for beta=.5, alpha=76.5

    else:
        p_red[3] = .1
        IC0 = array([ 0.97237007,  0.16336241,  1.07238146,  0.97376284,  0.44756444]) # for beta=.5, alpha=76.5
 
else:
    raise NotImplementedError("No valid selection - select solution 1,2, or 3")


ICe = IC0[[0,1,3,4]] # remove |v| -> this will be determined from the system energy
evs, IC = getPS(ICe, p_red, p_base, E_des, get_EV = True, maxnorm=1e-5, rcond=1e-7, 
                maxStep=.1, maxrep_inner=10, h=1e-4)
print IC
print "max. ev:", max(abs(evs))


    

In [ ]:

    

# transform IC's into appropriate formats
ICe_p = bslip.ICcircle_to_ICeuklid(IC)
ICcr_p = IC[[0,1,3,4]] # reduced (no "v") circular parameters

p = p_red[:] # shortcut
p_red8 = array([p[0], p[0], p[2], p[2], p[3], -p[3], 1., 1.])


    

In [ ]:

    

# simulate the model
ICe_p = bslip.ICcircle_to_ICeuklid(IC)        
mdl = bslip.BSLIP_newTD(bslip.pred_to_p(p_base, p_red8), ICe_p)
mdl.do_step(); mdl.do_step()
pass


    

In [ ]:

    

# compute "walking width"
seq = mdl.feet1_seq
v1 = array(seq[2]) - array(seq[0])
v2 = array(seq[1]) - array(seq[0])
v1 /= norm(v1)
ww = cross(v1, v2)
print "walking width: {:.5f} m".format(norm(ww[1]))


    

In [ ]:

    

# change delta l0 
# re-compute periodic solution
# check stability
ICcr_p = IC[[0,1,3,4]]
delta_l0 = 0.
pns = []
while True:
    p8 = p_red8.copy()
    delta_l0 += .00025
    p8[6] += delta_l0
    p8[7] -= delta_l0
    try:
        evs, IC_new = getPS(ICcr_p, p8, p_base, E_des, get_EV = True, maxnorm=1e-6, rcond=1e-7,  maxStep=.1, 
                    maxrep_inner=10, h=1e-4)
    except Exception:
        print "(FAILED)"
        # revert to last stable solution (e.g. for visualization)
        p8[6] -= 0.00025
        p8[7] += 0.00025
        break
    if max(abs(evs)) >= 1.0:
        print "(UNSTABLE)"
        break
    else:
        ICcr_p = IC_new[[0,1,3,4]]
        sys.stdout.write('x')
        
        ICe_p = bslip.ICcircle_to_ICeuklid(IC_new)
        ICc_p = array(IC_new).squeeze()
        
        mdl = bslip.BSLIP_newTD(bslip.pred_to_p(p_base, p8), ICe_p)
        mdl.do_step(); mdl.do_step()
        fs = array(mdl.state)
        fs[:3] -= array(mdl.feet1_seq[-1])
        ICc1 = bslip.ICeuklid_to_ICcircle(fs)
        pns.append(norm(ICc1 - array(ICc_p)))
        

if selection==1:
    pname = "A"
elif selection==2:
    pname = "B"
else:
    pname ="C"
bname = "0.05" if not bbeta else "0.1"
print "Point: {}, Beta: {} |-> Delta l0: {:.4f}".format(pname, bname, delta_l0)


    

In [ ]:

    

# change delta l0 
# re-compute periodic solution
# check stability
ICcr_p = IC[[0,1,3,4]]
delta_beta = 0.
pns = []
stepping = 0.005
while True:
    p8 = p_red8.copy()
    delta_beta += stepping
    p8[4] += delta_beta
    p8[5] -= delta_beta
    try:
        evs, IC_new = getPS(ICcr_p, p8, p_base, E_des, get_EV = True, maxnorm=1e-6, rcond=1e-7,  maxStep=.1, 
                    maxrep_inner=10, h=1e-4)
    except Exception:
        print "(FAILED)"
        # revert to last stable solution (e.g. for visualization)
        p8[4] -= stepping
        p8[5] += stepping
        break
    if max(abs(evs)) >= 1.0:
        print "(UNSTABLE)"
        break
    else:
        ICcr_p = IC_new[[0,1,3,4]]
        sys.stdout.write('x')
        
        ICe_p = bslip.ICcircle_to_ICeuklid(IC_new)
        ICc_p = array(IC_new).squeeze()
        
        mdl = bslip.BSLIP_newTD(bslip.pred_to_p(p_base, p8), ICe_p)
        mdl.do_step(); mdl.do_step()
        fs = array(mdl.state)
        fs[:3] -= array(mdl.feet1_seq[-1])
        ICc1 = bslip.ICeuklid_to_ICcircle(fs)
        pns.append(norm(ICc1 - array(ICc_p)))
        

if selection==1:
    pname = "A"
elif selection==2:
    pname = "B"
else:
    pname ="C"
bname = "0.05" if not bbeta else "0.1"
print "Point: {}, Beta: {} |-> Delta beta: {:.4f}".format(pname, bname, delta_beta)


    

In [ ]:

    

# verify periodicity - actually this is only for bugfixing
#p8
ICe_p = bslip.ICcircle_to_ICeuklid(IC_new)
ICc_p = array(IC_new).squeeze()

mdl = bslip.BSLIP_newTD(bslip.pred_to_p(p_base, p8), ICe_p)
mdl.do_step(); mdl.do_step()
fs = array(mdl.state)
fs[:3] -= array(mdl.feet1_seq[-1])
ICc1 = bslip.ICeuklid_to_ICcircle(fs)

print ICc1 - array(ICc_p)


    

In [ ]:

    

# compute periodic solution (see step 3)
# create model with "old touchdown functionality"
# create circular model
# run circular model. compute beta w.r.t. x-direction
# verify periodicity of both models
# compute jacobians + eigenvalues


    

In [ ]:

    

import models.bslip as mbs


    

In [ ]:

    

# transform IC's into appropriate formats
ICe_p = bslip.ICcircle_to_ICeuklid(IC)
ICc_p = IC.copy()
#p = p_red[:] # shortcut
#p_red8 = array([p[0], p[0], p[2], p[2], p[3], -p[3], 1., 1.])

mdl = bslip.BSLIP_newTD(bslip.pred_to_p(p_base, p_red8), ICe_p)
mdl.do_step(); mdl.do_step()
fs = array(mdl.state)
fs[:3] -= array(mdl.feet1_seq[-1])
ICc1 = bslip.ICeuklid_to_ICcircle(fs)


    

In [ ]:

    

# compute "effective" beta (angle w.r.t. x-axis)
b1 = mdl.params['lp2'][-1] - arctan2(mdl.y_ss_seq[0][-1,5], mdl.y_ss_seq[0][-1,3] )
b2 = mdl.params['lp1'][-1] - arctan2(mdl.y_ss_seq[1][-1,5], mdl.y_ss_seq[1][-1,3] )

p_red8_x = p_red8.copy()
p_red8_x[4] = b2
p_red8_x[5] = b1

pper_ctd = bslip.pred_to_p(p_base, p_red8_x)
mdl2 = mbs.BSLIP(pper_ctd, ICe_p)
mdl2.do_step(); mdl2.do_step()
ref_pos = array(mdl2.state)
fs2 = array(mdl2.state)
fs2[:3] -= array(mdl2.feet1_seq[-1])
ICc2 = bslip.ICeuklid_to_ICcircle(fs2)

print "delta: 1: {}, 2: {} ".format(norm( ICc1 - array(ICc_p)), norm(ICc2 - array(ICc_p)))


    

In [ ]:

    

# compute jacobians
h = 1e-5

figure()
models = [lambda IC: mbs.BSLIP(pper_ctd, IC),
          lambda IC: bslip.BSLIP_newTD(bslip.pred_to_p(p_base, p_red8), IC)]
for model in models:
    
    J = []
    
    for dim in range(6):
        # "+"
        ICe_J = array(ICe_p).squeeze()
        ICe_J[dim] += h
        mdl2 = model(ICe_J)
        mdl2.do_step(); mdl2.do_step();
        fsp = mdl2.state.copy()
        fsp[:3] -= mdl2.feet1_seq[-1]
    
        # "-"
        ICe_J = array(ICe_p).squeeze()
        ICe_J[dim] -= h
        mdl2 = model(ICe_J)
        mdl2.do_step(); mdl2.do_step();
        fsm = mdl2.state.copy()
        fsm[:3] -= mdl2.feet1_seq[-1]
    
        J.append((fsp - fsm) / (2*h))
    
        
    J = vstack(J)
    
    J_evs = abs(eig(J)[0])
    J_evs.sort()
    plot(J_evs,'d--')
    print ' '.join(['{:.4f}'.format(val) for val in J_evs])
    
plot([-1,6],[1,1],'k--')
ylim(0,6)
pass


    

In [ ]:

    

# A1: beta=.1, dk = -2, da = -2 # modify manually such that r-> inf
# A2: beta=.1, dk = -1, da = -5 # s.th. r = 17.6
# A3: beta=.1, dk =-1, da = 4 # s.th. r = -11.7
# B1: beta=.1, dk = -2, da = -9.5 # s.th. r= 7.8
# C1: beta=.05, dk=-8, da=1.3 # s th r=-35.3  
c


    

In [ ]:

    

# --- select solution properties


# "A1" (r=5000) 
# result: is not "straight" anymore when energy changes (!!)
c.vis_beta = .1
c.vis_dk = 2050
c.vis_da = -2 * pi / 180

# **NOTE** for "very" high dE, these are no "normal" walking solutions any more - the have aerial phases! (-> exclude them)

# "A2"
# velocity and energy anti-correlated (!) for most part;  r rising with speed
#c.vis_beta = .10 # one out of .05, .10
#c.vis_da = -4.7 * pi / 180
#c.vis_dk = 1000 # sign flip in here!

# "A3"
# similar to A2; radius < 0 -> magnitude rises with speed
#c.vis_beta = .10 # one out of .05, .10
#c.vis_da = 4.7 * pi / 180
#c.vis_dk = 1000 # sign flip in here!

# "B1"
# same as A2, A3
#c.vis_beta = .1
#c.vis_da = -9.5 * pi / 180
#c.vis_dk = 2000

# "C1"
#
#c.vis_beta = .05 # one out of .05, .10
#c.vis_da = 1.3 * pi / 180
#c.vis_dk = 8000 # sign flip in here!





# --- prepare block
# --- --- import functions
# import models.bslip as bslip

# --- --- define useful helper function
def find_idx(dk, da, sols):
    """
    returns the solution index which most closely matches dk, da
    
    """
    #dk = array([x[1][0] - x[1][1] for x in dat1])
    #da = array([x[1][2] - x[1][3] for x in dat1])
    dk_ref = 1e9
    da_ref = 1e9
    idx_ref = 0
    for idx, sol in enumerate(sols):
        dk_f = sol[1][0] - sol[1][1]
        da_f = sol[1][2] - sol[1][3]
        if abs(da_f - da) <= da_ref:
            if abs(abs(dk_f) - dk) <= dk_ref:
                da_ref = abs(da_f - da)
                dk_ref = abs(abs(dk_f) - dk)
                idx_ref = idx
                                
    return idx_ref
        
# --- identify closest solution in dataset
if c.point == "A":
    pr0 = array([14000, 14000, 69.*pi / 180, 69 * pi / 180, .05, -.05, 1., 1.])
elif c.point == "B":
    pr0 = array([14000, 14000, 72.5 *pi / 180, 72.5 * pi / 180, .05, -.05, 1., 1.])
elif c.point == "C":
    pr0 = array([20000, 20000, 76.5 *pi / 180, 76.5 * pi / 180, .05, -.05, 1., 1.])
else:
    raise ValueError("Wrong value for c.point (A, , or C)")
        
pr0[4] = c.vis_beta
pr0[5] = -c.vis_beta
if c.vis_beta == .05:
    dat = dat1
elif c.vis_beta == .1:
    dat = dat2
else:
    raise ValueError("Wrong value for c.vis_beta (.05 or .1)")

# --- --- find best solution, get corresponding parameters and IC
idx = find_idx(c.vis_dk, c.vis_da, dat)
dk, da = -diff(dat[idx][1])[[0,2]]
ICc = dat[idx][0]

# --- --- update model parameters
pr0a = pr0.copy()
pr0a[2] += da / 2.
pr0a[3] -= da / 2.
pr0a[0] += dk / 2.
pr0a[1] -= dk / 2.

pbase = bslip.demo_p
pbase['delta_beta'] = .0
P = bslip.pred_to_p(pbase, pr0a)


# --- --- some debug output
print "idx = ", idx 
print "dk:", dk, "(", c.vis_dk, ")"
print "da:", da * 180 / pi, "(", da, ") (", c.vis_da, ")"
print "r:", dat[idx][2]


    

In [ ]:

    

IC0 = dat[idx][0][[0,1,3,4]].copy()
E_des = 816.

#ICcr_p = IC[[0,1,3,4]]
#delta_beta = 0.
#pns = []
#stepping = 0.005
#while True:
#    p8 = p_red8.copy()
#    delta_beta += stepping
#    p8[4] += delta_beta
#    p8[5] -= delta_beta
#    try:
#        evs, IC_new = getPS(ICcr_p, p8, p_base, E_des, get_EV = True, maxnorm=1e-6, rcond=1e-7,  maxStep=.1, 
#                    maxrep_inner=10, h=1e-4)

evs, IC = getPS(IC0, dat[idx][1], pbase, E_des, get_EV = True, maxnorm=1e-5, rcond=1e-7, 
                maxStep=.1, maxrep_inner=10, h=1e-4)


    

In [ ]:

    

import scipy.integrate as si

def getSpeed(ICc, pred, pbase):
    """
    returns (v1, v2)
    where v1 = |end - start| / T, and v2 = 1/T INTEGRAL(v)
    """
    ICe = bslip.ICcircle_to_ICeuklid(ICc)
    mdl = bslip.BSLIP_newTD(bslip.pred_to_p(pbase, pred), ICe)
    mdl.do_step()
    mdl.do_step()
    dst = norm(mdl.y_ds_seq[1][1,[0,2]] - mdl.y_ss_seq[0][0,[0,2]])
    t_tot = mdl.t_ss_seq[0][-1] + mdl.t_ss_seq[1][-1] + mdl.t_ds_seq[0][-1] + mdl.t_ds_seq[1][-1]
    
    #mdl.y_ds_seq
    y_tot = vstack([mdl.y_ss_seq[0], mdl.y_ds_seq[0], mdl.y_ss_seq[1], mdl.y_ds_seq[1]] )
    ttime = []
    for time_ in [mdl.t_ss_seq[0], mdl.t_ds_seq[0], mdl.t_ss_seq[1], mdl.t_ds_seq[1]]:
        if len(ttime):
            ttime.append(time_ + ttime[-1][-1])
        else:
            ttime.append(time_)
    ttime = hstack(ttime)
    ix = si.cumtrapz(y_tot[:,3], ttime)[-1]
    iy = si.cumtrapz(y_tot[:,5], ttime)[-1]
    t_dist = sqrt(ix**2 + iy**2)
    
    
    return (dst / t_tot, t_dist / t_tot)


    

In [ ]:

    

all_r = []
all_E = []
all_v = []
res = [] # (E, r)
IC = dat[idx][0][[0,1,3,4]].copy()
E_des = 816.
pred = dat[idx][1]

deltaE = 1. if c.point != "C" else .1 # .1 for "C" type

nm = 0
try:
    while True:
        evs, IC0 = getPS(IC, dat[idx][1], pbase, E_des, get_EV = True, maxnorm=1e-5, rcond=1e-7, 
                    maxStep=.1, maxrep_inner=10, h=1e-4)
        if max(abs(evs)) > 1:
            break
        diam = getR(IC0, dat[idx][1], pbase)
        speed1, speed2 = getSpeed(IC0, dat[idx][1], pbase)
        res.append([E_des, diam, speed1, speed2])
        E_des -= .1
        IC = IC0[[0,1,3,4]]
        sys.stdout.write('.')
        nm += 1
except Exception:    
    # failed -> continue with other direction
    pass

r1 = vstack(res)[::-1,:]
res = []
IC = dat[idx][0][[0,1,3,4]].copy()
E_des = 816.
try:
    while True:
        evs, IC0 = getPS(IC, dat[idx][1], pbase, E_des, get_EV = True, maxnorm=1e-5, rcond=1e-7, 
                    maxStep=.1, maxrep_inner=10, h=1e-4)
        if max(abs(evs)) > 1:
            break
        diam = getR(IC0, dat[idx][1], pbase)    
        speed1, speed2 = getSpeed(IC0, dat[idx][1], pbase)
        res.append([E_des, diam, speed1, speed2])
        E_des += .1
        IC = IC0[[0,1,3,4]]
        sys.stdout.write('.')
except Exception:
    # failed -> continue with other direction
    pass
r2 = vstack(res)


    

In [ ]:

    




    

In [ ]:

    

res = vstack([r1, r2])

figure(figsize=(12,6))
subplot(1,2,1)
plot(res[:,2], res[:,1],'g.-')
plot(res[nm,2], res[nm,1],'rd')
title('radius over velocity')
xlabel('velocity [m/s]')
ylabel('radius [m]')
#ylim(-500,500)
subplot(1,2,2)
plot(res[:,0], res[:,1],'b.-')
plot(res[nm,0], res[nm,1],'rd')
title('radius over energy')
xlabel('energy [J]')
ylabel('radius [m]')
#ylim(-500,500)


    

In [ ]: